#include "SimulationThread.h"

namespace jcrada {

    SimulationThread::SimulationThread(BulletSimulator& simulator) :
    _stopped(true),
    _simulation_speed(100),
    _simulator(&simulator),
    _iterations(-1),
    _timer(new QTime()),
    _behavior_enabled(false),
    _behavior_on_ground(false),
    _current_iteration(0) {
    }

    SimulationThread::~SimulationThread() {
        stop();
        delete _timer;
    }

    void SimulationThread::run() {
        emit started();
        QTime timer;
        timer.start();
        int iterations = getIterations();
        if (iterations < 0) {
            while (!isStopped()) {
                iterate();
            }
        } else {
            for (int i = 0; i < iterations && !isStopped(); ++i) {
                iterate();
            }
        }
        setStopped(true);
        emit stopped();
        emit message(QString(("Time Elapsed: " + TO_STRING(timer.elapsed() / 1000.0) + " s.").c_str()));
    }

    void SimulationThread::play() {
        if (!isRunning()) {
            setStopped(false);
            start();
        }
    }

    void SimulationThread::stop() {
        int speed = getSimulationSpeed();
        setSimulationSpeed(0);
        setStopped(true);
        wait(1000);
        while (isRunning()) {
            emit message("Waiting for simulation to finish...");
            wait(1000);
        }
        setSimulationSpeed(speed);
    }

    void SimulationThread::ensureFrameRate(int time_elapsed_in_milliseconds) {
        long sleep = (1000.0 / getSimulator().getTimestepsPerSecond()) - time_elapsed_in_milliseconds;
        if (sleep > 0) {
            this->msleep((getSimulationSpeed() / 100.0) * sleep);
        } else {
            BDEBUG("Warning: Too many timesteps per second");
        }
    }

    void SimulationThread::performStep() {
        getSimulator().getLock().lockForRead();
        Creature* creature = getSimulator().currentCreature();
        if (creature && isBehaviorEnabled()) {
            if (!isBehaviorOnGround() ||
                    (isBehaviorOnGround() && getSimulator().hasHitTheGround())) {
                creature->onStep();
            }
        }
        getSimulator().getLock().unlock();
        getSimulator().step();
    }
}
